/*!
  @file feature.h
  @copyright 2013 Kubota Lab. All rights resereved.
*/

#ifndef _FEATURE_H_
#define _FEATURE_H_

#include <vector> 

#include <boost/shared_ptr.hpp>

#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp> 
#include <opencv2/nonfree/features2d.hpp> 

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

namespace vo {

/*! 
 *  @brief  Feature data container
 *  @author Kyohei Otsu <kyon@ac.jaxa.jp>
 *  @date   2013
 *  
 *  detailed documentaiton for this class
 */

class Feature {
 public:
  typedef boost::shared_ptr<Feature> Ptr;
  typedef boost::shared_ptr<Feature const> ConstPtr;

  typedef std::vector<cv::KeyPoint> KeyPoints;
  typedef cv::Mat Descriptors;
  typedef std::vector<cv::DMatch> Matches;
  typedef std::vector<cv::Point3d> ScenePoints;
  typedef pcl::PointXYZ PointType;
  typedef pcl::PointCloud<PointType> Cloud;

  Feature()
      : kp_vec(2)
      , desc_vec(2)
      , matches(new Matches)
      , sp(new ScenePoints)
      , frame_id(-1)
  { 
    kp_vec[0].reset(new KeyPoints);
    kp_vec[1].reset(new KeyPoints);
    desc_vec[0].reset(new Descriptors);
    desc_vec[1].reset(new Descriptors);
  }

  ~Feature() {}


  //! Gets a pointcloud of scene points
  void getCloud(Cloud &cloud) {
    cloud.width = sp->size();
    cloud.height = 1;
    cloud.is_dense = false;
    cloud.points.resize(cloud.width * cloud.height);

    for (size_t i = 0; i < sp->size(); i++) {
      cloud.points[i].x = sp->at(i).x;
      cloud.points[i].y = sp->at(i).y;
      cloud.points[i].z = sp->at(i).z;
    }
  }


  std::vector< boost::shared_ptr<KeyPoints> > kp_vec;
  std::vector< boost::shared_ptr<Descriptors> > desc_vec;
  boost::shared_ptr<Matches> matches;
  boost::shared_ptr<ScenePoints> sp;  //!< in left camra coordinate frame
  int frame_id;
};

}  // namespace vo

#endif
